#include "GoInStraightLineState.h"
#include "Vector2.h"
#include "PDController.h"

GoInStraightLineState::GoInStraightLineState(void)
{
	//points[0] = Vector2(0, 150);
	//points[1] = Vector2(0, -150);
	points[0] = Vector2(-200, -350);
	points[1] = Vector2(-200, 350);
	pos = 0;
}


GoInStraightLineState::~GoInStraightLineState(void)
{
}

Command GoInStraightLineState::GetCommand(Agent* agent) {
	Vector2 agentPos = Vector2(agent->getTank()->pos[0],agent->getTank()->pos[1]);
	if((agentPos-points[pos]).magnitude() < 40)
	{
		pos ++;
		if(pos == 2) pos = 0;
	}
	
	return PDController::instance()->GetComand(agent->getTank(), (points[pos]-agentPos));
	//return Command(agent->getTank()->index, 1, 1, false);
}
